#include "GeoSurfaceCutter.h"

using namespace H3D;

Vec3f GeoSurfaceCutter::GetNormal(GeoCuttableSurface::ShapeDatabase::Point * pt)
{
	return Vec3f(1,0,0);
}

Vec3f GeoSurfaceCutter::GetNormal(GeoCuttableSurface::ShapeDatabase::Face * fc)
{
	return Vec3f(1,0,0);
}

SpringCutter::SpringCutter()
{
}

SpringCutter::~SpringCutter()
{
}

bool areAdjacent(GeoCuttableSurface::ShapeDatabase::Point & a, GeoCuttableSurface::ShapeDatabase::Point & b)
{
	for(int i = 0; i<a.edges.size(); i++)
	{
		if(a.edges[i]->endpoints[0] == &b || a.edges[i]->endpoints[1] == &b)
		{
			return true;
		}
	}

	return false;
}

bool CrossesPlane(Vec3f P1, Vec3f P2, double a, double b, double c, double d, Vec3f * intersection_p)
{
    double alpha, t;
    
    alpha = a * (P1.x - P2.x) + b * (P1.y - P2.y) + c * (P1.z - P2.z);
    if(alpha == 0) return false;
    
    t = -(a * P2.x + b * P2.y + c * P2.z + d) / alpha;
    if ((t < 0) || (t > 1)) 
		return false;
    
    if (intersection_p) {
        intersection_p->x = (P1.x - P2.x) * t + P2.x;
        intersection_p->y = (P1.y - P2.y) * t + P2.y;
        intersection_p->z = (P1.z - P2.z) * t + P2.z;
    }
    
    return true;
}

void DuplicatePath(GeoCuttableSurface::ShapeDatabase::Point** path, int num_nodes_on_path, GeoCuttableSurface::ShapeDatabase & db, vector<Vec3f> & coords)
{
	//TODO: JDS Make this false and/or remove it
	bool debug = true;
    if (debug) 
        cerr << "Object::DuplicatePath(" << path << "," << num_nodes_on_path 
        << ")\n";
    
    // sanity check
    if (!path || (num_nodes_on_path <= 0)) return;
    //SanityCheck();
    
    { // Color the mesh along path
        for (int i=0; i<num_nodes_on_path-1; i++)
            path[i]->ColorMeshForSplitAtNode(path[i+1], db, coords);
    }
    
    // another check
    cerr << "After ColorMeshForSplit:\n";
    //SanityCheck();
    
    { // split the mesh along path, using the coloring
        for (int i=0; i<num_nodes_on_path-1; i++)
            path[i]->SplitColoredMeshAtNode(path[i+1], db, coords);
    }
    
    // another check
    cerr << "After DuplicateMesh:\n";
    //SanityCheck();
}

bool SpringCutter::Cut(GeoCuttableSurface::ShapeDatabase &db, vector<Vec3f> & coords, GeoCuttableSurface::ShapeDatabase::Point **pts, int numPts)
{
	int num_nodes_in_path = 0;
    const int max_nodes_in_path = 10000;
	GeoCuttableSurface::ShapeDatabase::Point* path[max_nodes_in_path];
    
    //loop for each pair of markers
	for (int i=0; i<numPts-1; i++) {
        // get local pointers
        GeoCuttableSurface::ShapeDatabase::Point* s = pts[i];
		GeoCuttableSurface::ShapeDatabase::Point* e = pts[i+1];

		//std::cout<<coords[s->index]<<" "<<coords[e->index]<<endl;
        
        // add s to the path
        path[num_nodes_in_path] = s;
        if (num_nodes_in_path < max_nodes_in_path)
            num_nodes_in_path++;
        
		//Mike Oren: Conducting tests with coordinates...
        // get the plane equation for the face
        double a, b, c, d;
        b = coords[s->index].y - coords[e->index].y;
		if(b > coords[s->index].y && b > coords[e->index].y)
			b = coords[e->index].y - coords[s->index].y;
		c = 0.0;//coords[s->index].z - coords[e->index].z;
		a = coords[s->index].x - coords[e->index].x;
		if(a > coords[s->index].x && a > coords[e->index].x)
			a = coords[e->index].x - coords[s->index].x;
		d = -(a * coords[s->index].x + b * coords[s->index].y);

		//double a, b, c, d;
  //      b = coords[s->index].y - coords[coords.size()-1].y;
		//if(b > coords[s->index].y && b > coords[coords.size()-1].y)
		//	b = coords[coords.size()-1].y - coords[s->index].y;
		//c = 0.0;//coords[s->index].z - coords[e->index].z;
		//a = coords[s->index].x - coords[coords.size()-1].x;
		//if(a > coords[s->index].x && a > coords[coords.size()-1].x)
		//	a = coords[coords.size()-1].x - coords[s->index].x;
		//d = -(a * coords[s->index].x + b * coords[s->index].y);
        
        // loop, crossing faces

		//Mike Oren: This is where the endless loop occurs when it occurs.
		while (! areAdjacent(*s, *e)) {            
            // print out S and E
            cerr << "====================================\n";
            cerr << "S is " << s << endl;
            cerr << "E is " << e << endl;

			std::cout<<coords[s->index].x<<" ";
			std::cout<<coords[s->index].y<<" ";
			std::cout<<coords[s->index].z<<" | ";
			std::cout<<coords[e->index].x<<" ";
			std::cout<<coords[e->index].y<<" ";
			std::cout<<coords[e->index].z<<endl;
            
            // find which face using s to start with
			GeoCuttableSurface::ShapeDatabase::Face* cur_face = NULL; 
			GeoCuttableSurface::ShapeDatabase::Edge* crossing_edge = NULL;
            Vec3f intersection_pt; double min_dist = 1e10;

			for (int i=0; i<s->faces.size(); i++) {
				GeoCuttableSurface::ShapeDatabase::Face* test_face = s->faces[i];
                
                // find the edge of test_face that does not have s
                for (int j=0; j<3; j++) {
					GeoCuttableSurface::ShapeDatabase::Edge* cur_edge = test_face->edges[j];
					if (cur_edge->endpoints[0] == s) continue;
                    if (cur_edge->endpoints[1] == s) continue;
                    
                    // got an edge not using s
                    Vec3f cur_intersect_pt;
					if (!CrossesPlane(coords[cur_edge->endpoints[0]->index],
									  coords[cur_edge->endpoints[1]->index],
									  a,b,c,d, &cur_intersect_pt)) 
                        continue;
                    
					std::cout<<coords[cur_edge->endpoints[0]->index][0]<<" ";
					std::cout<<coords[cur_edge->endpoints[0]->index][1]<<" ";
					std::cout<<coords[cur_edge->endpoints[0]->index][2]<<" | ";
					std::cout<<coords[cur_edge->endpoints[1]->index][0]<<" ";
					std::cout<<coords[cur_edge->endpoints[1]->index][1]<<" ";
					std::cout<<coords[cur_edge->endpoints[1]->index][2]<<" | ";
					std::cout<<cur_intersect_pt[0]<<" "<<cur_intersect_pt[1]<<" "<<cur_intersect_pt[2]<<endl;
					std::cout<<"******* **********"<<endl;
					std::cout<<a<<" "<<b<<" "<<c<<" "<<d<<endl;
					std::cout<<"--------------"<<endl;

                    // got one of the two edges that cross plane

					//Mike Oren: More coordinate based tests.
                    double cur_dist = (cur_intersect_pt - coords[e->index]).lengthSqr();
					//double cur_dist = (cur_intersect_pt - coords[coords.size()-1]).lengthSqr();
                    if (cur_dist < min_dist) {
                        crossing_edge = cur_edge;
                        min_dist = cur_dist;
                        intersection_pt = cur_intersect_pt;
                        cur_face = test_face;
                    }
                }
            }
            
            // sanity checks
			if (!cur_face) {
				cerr << "No face found!" << endl;
				break;
			}
			if (!crossing_edge) {
				cerr << "No crossing edge found!" << endl;
				break;
			}
            
            // debugging
            cerr << "start node = " << s << endl;
            cerr << "cur_face is " << cur_face << endl;
            cerr << "crossing_edge is " << crossing_edge << endl;
            
            cerr << "crossing edge is " << crossing_edge << endl;
            cerr << "intersection_pt = " << intersection_pt.x << ","
                << intersection_pt.y << "," << intersection_pt.z << endl;
            
            // subdivide face along s and intersection point (THIS KILLS cur_face!)
            cerr << "first subdivide\n";
			GeoCuttableSurface::ShapeDatabase::Point* new_node = GeoCuttableSurface::ShapeDatabase::subdivideFace(cur_face, intersection_pt, db, coords);
            cerr << "end first subdivide\n";
            
            // sanity check- if subdivide failed
            if (!new_node) { 
				cerr << "first subdivide failed!\n"; 
				break; }
            
            // test if we have reached e
			if (areAdjacent(*new_node, *e)) {
				cerr << "Are adjacent!\n";
				break;
			}
            
            // do a sanity check to verify links, etc
            //SanityCheck();
            
            // note- we have split faces on one side (and thereby created new
            // subedges), but have not the other side of the main edge
            
            // if still have a face attached to the edge, must've had 2 faces
			GeoCuttableSurface::ShapeDatabase::Face* other_face = NULL;
			if(crossing_edge->faces[0] != NULL)
			{
				other_face = crossing_edge->faces[0];
			}
			else if(crossing_edge->faces[1] != NULL)
			{
				other_face = crossing_edge->faces[1];
			}

            if (other_face != NULL ) {
				//GeoCuttableSurface::ShapeDatabase::Face* other_face = crossing_edge->faces[0];
                // also subdivide other face of edge at same pt 
                cerr << "second subdivide\n";
                GeoCuttableSurface::ShapeDatabase::subdivideFace(other_face, new_node, db, coords);
                cerr << "end second subdivide\n";
            }
            else cerr << "no other face\n";
            
            // either way, can get rid of the original edge
			db.DeleteEdge(crossing_edge);
            
            // save new_node to the path
            path[num_nodes_in_path] = new_node;
			cerr<<"Num nodes in path: "<<num_nodes_in_path<<endl;
            if (num_nodes_in_path < max_nodes_in_path)
                num_nodes_in_path++;
            
            // and jump to next node
            s = new_node; //M.O.: This isn't the correct node to set to s?
        }
  } // for i
  
  // do a sanity check to verify links, etc
  //SanityCheck();
  
  { // lastly, duplicate the path we created
      for (int i=0; i<numPts-1; i++) 
          DuplicatePath(path, num_nodes_in_path, db, coords);
  }
  
  // do a final sanity check to verify links, etc
  //SanityCheck();
  
  // reset the colors we used

  set<GeoCuttableSurface::ShapeDatabase::Point *>::iterator pIterator;
  for(pIterator = db.points.begin(); pIterator != db.points.end(); pIterator++)
  {
	  (*pIterator)->setPartId(0);
  }

	return true;
}

DeletionCutter::DeletionCutter()
{
}

DeletionCutter::~DeletionCutter()
{
}

bool DeletionCutter::Cut(GeoCuttableSurface::ShapeDatabase &db, vector<Vec3f> & coords, GeoCuttableSurface::ShapeDatabase::Point **pts, int numPts)
{
	return false;
}